#include "ocvtools.h"
#include "hsvcolor.h"

#include <QtGui>
#include <cv.h>

HSVColor OCVTools::pixelColor(IplImage *hsvImage, int x, int y)
{
    int h = (uchar)hsvImage->imageData[y * hsvImage->widthStep + x * hsvImage->nChannels]*2;

    if (h < 0)
        h += 255;

    int s = (uchar)hsvImage->imageData[y * hsvImage->widthStep + x * hsvImage->nChannels + 1];
    int v = (uchar)hsvImage->imageData[y * hsvImage->widthStep + x * hsvImage->nChannels + 2];

    return HSVColor(h,s,v);
}

bool OCVTools::isPixelInRange(IplImage *hsvImage, const HSVColor &refColor, int x, int y)
{
    int pos = y * hsvImage->widthStep + x * 3;

    if ( abs((uchar)hsvImage->imageData[pos + 1] - refColor.s()) > refColor.sr() )
        return false;

    if ( abs((uchar)hsvImage->imageData[pos + 2] - refColor.v()) > refColor.vr() )
        return false;

    int rangeH = refColor.hr();
    int refH = refColor.h();
    int pixelH = (uchar)hsvImage->imageData[pos]*2;
    if (pixelH < 0)
        pixelH += 255;

    if ( abs(pixelH - refH) > rangeH )
    {

        if ( refH + rangeH > 359 && pixelH < refH )
            if ( pixelH <= (refH + rangeH - 359) )
                return true;

        if ( refH - rangeH < 0 && pixelH > refH )
            if ( pixelH >= refH - rangeH + 359)
                return true;

        return false;
    }

    return true;
}

int OCVTools::maskMatchingPixels(IplImage *hsvImage, IplImage *destImage, const HSVColor &refColor)
{
    int count = 0;

    for (int xPos=0; xPos < (int)hsvImage->width; ++xPos)
    {
        for (int yPos=0; yPos < (int)hsvImage->height; ++yPos)
        {
            if ( isPixelInRange(hsvImage, refColor, xPos, yPos) )
            {
                maskPixel(destImage, xPos, yPos, 255, 0, 0);
                count++;
            }
        }
    }

    return count;
}

void OCVTools::createMask(IplImage *hsvImage, IplImage *mask, const HSVColor &refColor)
{
    for (int xPos=0; xPos < (int)hsvImage->width; ++xPos)
    {
        for (int yPos=0; yPos < (int)hsvImage->height; ++yPos)
        {
            if ( !isPixelInRange(hsvImage, refColor, xPos, yPos) )
            {
                mask->imageData[yPos * mask->widthStep + xPos] = 1;
            }
        }
    }
}

void OCVTools::maskPixel(IplImage *destImage, int x, int y, int r, int g, int b)
{
    ( destImage->imageData[y * destImage->widthStep + x * destImage->nChannels]) = b;
    ( destImage->imageData[y * destImage->widthStep + x * destImage->nChannels + 1]) = g;
    ( destImage->imageData[y * destImage->widthStep + x * destImage->nChannels + 2]) = r;
}

void OCVTools::createChessboard(IplImage *chessboard)
{
    int fieldSize = 20;
    bool dark = true;
    int color = 50;

    int count = 0;

    for (int i=0; i < ((int)chessboard->width*(int)chessboard->height)/fieldSize; ++i)
    {
        for (int y=0; y < fieldSize; ++y)
        {
            chessboard->imageData[count*3] = color;
            chessboard->imageData[count*3+1] = color;
            chessboard->imageData[count*3+2] = color;

            count++;
        }

        if (!(count%((int)chessboard->width*fieldSize)))
            dark = !dark;

        dark = !dark;
        color = dark ? 90 : 120;
    }
}

